//
// Created by yongj on 2021/7/19.
//

#include <string.h>
#include "uart.h"

void uartINIT() {
    //初始化串口并设置波特率
    uart_init(UART_ID, BAUD_RATE);

    //设置GPIO功能
    gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
    gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);

    //设置 CTS/RTS 状态
    uart_set_hw_flow(UART_ID, false, false);

    //设置数据格式
    uart_set_format(UART_ID, DATA_BITS, STOP_BITS, PARITY);

    //禁用FIFO
    uart_set_fifo_enabled(UART_ID, false);

    irq_set_exclusive_handler(UART0_IRQ, uartREAD);
    irq_set_enabled(UART0_IRQ, true);

    //设置串口中断        允许RX中断
    uart_set_irq_enables(UART_ID, true, false);

    uart_puts(UART_ID, "Hello, UART!\n");
}

void uartREAD() {
    static uint8_t num = 0;
    uint8_t buf[50];
    char *motorID_buf;
    static uint16_t motorID;
    char *KP_data;
    char *KI_data;
    char *KD_data;
    char *speed_data;
    if (uart_is_readable(UART_ID)) {
        if ((*(buf + num++) = uart_getc(UART_ID)) == '\n') {
            num = 0;
            printf("\n\nrec %s\n", buf);
            if ((motorID_buf = strstr((char *) buf, "ID")) != NULL) {
                motorID = *(motorID_buf+2)=='1' ? MOTOR_1_ID:MOTOR_2_ID;
            }
            if ((KP_data = strstr((char *) buf, "KP")) != NULL) {
                float Kp[4] = {*(KP_data + 2) - '0', *(KP_data + 3) - '0', *(KP_data + 4) - '0', 0};
                Kp[3] = Kp[0] + Kp[1] / 10 + Kp[2] / 100;
                printf("Kp set%.2f\n", Kp[3]);
                PID_set_Kp(Kp[3]);
            }
            if ((KI_data = strstr((char *) buf, "KI")) != NULL) {
                float ki[5] = {*(KI_data + 2) - '0', *(KI_data + 3) - '0', *(KI_data + 4) - '0', *(KI_data + 5) - '0',
                               0};
                ki[4] = ki[0] + ki[1] / 10 + ki[2] / 100 + ki[3] / 1000;
                printf("ki set%.2f\n", ki[4]);
                PID_set_Ki(ki[4]);
            }
            if ((KD_data = strstr((char *) buf, "KD")) != NULL) {
                float kd[5] = {*(KD_data + 2) - '0', *(KD_data + 3) - '0', *(KD_data + 4) - '0', *(KD_data + 5) - '0',
                               0};
                kd[4] = kd[0] + kd[1] / 10 + kd[2] / 100 + kd[3] / 1000;
                printf("kd set%.2f\n", kd[4]);
                PID_set_Kd(kd[4]);
            }
            if ((speed_data = strstr((char *) buf, "speed")) != NULL) {
                int speed = (*(speed_data + 6) - '0') * 10000 +
                             (*(speed_data + 7) - '0') * 1000 +
                             (*(speed_data + 8) - '0') * 100 +
                             (*(speed_data + 9) - '0') * 10 +
                             (*(speed_data + 10) - '0');
//                p36_set_motor_current(0x201, (*(speed_data + 5) != '-') ? speed : -speed);
                printf("speed set%d\n", (*(speed_data + 5) != '-') ? (int16_t) speed : -((int16_t) speed));
                p36_set_motor_RPM(motorID, (*(speed_data + 5) != '-') ? (int16_t) speed : -((int16_t) speed));
                p36_put_motor_current();
            }
            if ((strstr((char *) buf, "STOP")) != NULL) {
                p36_state_set_STOP();
            }
            memset(buf, 0, sizeof(buf));
        }
    }
}